#include <robot_chassis/CRobotControl.hpp>
#include <robot_chassis/CRobotImu.hpp>
#include <robot_chassis/CRobotOdom.hpp>
#include <robot_chassis/CRobotVoltage.hpp>
#include <robot_chassis/CTurnOnRobot.hpp>

/**
 * @brief 机器人驱动主入口
 * 
 * 该节点负责：
 * - 与STM32建立串口通信
 * - 发布里程计数据 (/chassis/odom) 和 TF 变换 (odom -> base_link)
 * - 发布IMU数据 (/chassis/imu) 和 TF 变换 (base_link -> imu_link)
 * - 发布电池状态 (/chassis/battery_state)
 * - 订阅速度指令 (/chassis/cmd_vel)
 */
int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);

    // 创建参数节点用于读取串口配置
    auto paramNode = std::make_shared<rclcpp::Node>("robot_driver_params");

    // 声明参数
    paramNode->declare_parameter<std::string>("serial_port", "/dev/ttyACM0");
    paramNode->declare_parameter<int>("baud_rate", 115200);
    paramNode->declare_parameter<int>("timeout_ms", 1000);

    // 获取参数
    std::string serialPort = paramNode->get_parameter("serial_port").as_string();
    int         baudRate   = paramNode->get_parameter("baud_rate").as_int();
    int         timeoutMs  = paramNode->get_parameter("timeout_ms").as_int();

    RCLCPP_INFO(paramNode->get_logger(), "===============================================");
    RCLCPP_INFO(paramNode->get_logger(), "  麦克纳姆轮小车 ROS2 驱动");
    RCLCPP_INFO(paramNode->get_logger(), "===============================================");
    RCLCPP_INFO(paramNode->get_logger(), "串口配置:");
    RCLCPP_INFO(paramNode->get_logger(), "  - 端口: %s", serialPort.c_str());
    RCLCPP_INFO(paramNode->get_logger(), "  - 波特率: %d", baudRate);
    RCLCPP_INFO(paramNode->get_logger(), "  - 超时: %d ms", timeoutMs);
    RCLCPP_INFO(paramNode->get_logger(), "===============================================");

    // 配置串口参数
    CTurnOnRobot&    turnonRobot = CTurnOnRobot::getInstance();
    SUsartParameters usartParam;
    usartParam.usartPortName_ = serialPort;
    usartParam.usartBaudRate_ = baudRate;
    usartParam.timeout_       = static_cast<uint32_t>(timeoutMs);

    // 打开串口
    if (!turnonRobot.openUsart(usartParam)) {
        RCLCPP_ERROR(paramNode->get_logger(), "===============================================");
        RCLCPP_ERROR(paramNode->get_logger(), "串口打开失败！");
        RCLCPP_ERROR(paramNode->get_logger(), "请检查:");
        RCLCPP_ERROR(paramNode->get_logger(), "  1. 设备是否已正确连接");
        RCLCPP_ERROR(paramNode->get_logger(), "  2. 串口路径是否正确 (当前: %s)", serialPort.c_str());
        RCLCPP_ERROR(paramNode->get_logger(), "  3. 是否有访问权限 (可能需要添加到 dialout 组)");
        RCLCPP_ERROR(paramNode->get_logger(), "  4. 串口是否被其他程序占用");
        RCLCPP_ERROR(paramNode->get_logger(), "");
        RCLCPP_ERROR(paramNode->get_logger(), "诊断命令:");
        RCLCPP_ERROR(paramNode->get_logger(), "  ls -l /dev/ttyACM* /dev/ttyACM*  # 查看可用串口");
        RCLCPP_ERROR(paramNode->get_logger(), "  groups  # 查看当前用户组");
        RCLCPP_ERROR(paramNode->get_logger(), "  lsof %s  # 检查串口占用", serialPort.c_str());
        RCLCPP_ERROR(paramNode->get_logger(), "===============================================");
        rclcpp::shutdown();
        return -1;
    }

    // 启动接收线程
    turnonRobot.startReceive();

    // 等待数据稳定
    RCLCPP_INFO(paramNode->get_logger(), "等待STM32数据...");
    rclcpp::sleep_for(std::chrono::milliseconds(2000));

    // 检查使能状态
    uint8_t enableRobot;
    turnonRobot.getEnableRobot(enableRobot);

    if (enableRobot == 0) {
        RCLCPP_INFO(paramNode->get_logger(), "机器人使能成功!");
        RCLCPP_INFO(paramNode->get_logger(), "===============================================");
        RCLCPP_INFO(paramNode->get_logger(), "话题信息:");
        RCLCPP_INFO(paramNode->get_logger(), "  发布: /chassis/odom (nav_msgs/Odometry)");
        RCLCPP_INFO(paramNode->get_logger(), "  发布: /chassis/imu (sensor_msgs/Imu)");
        RCLCPP_INFO(paramNode->get_logger(), "  发布: /chassis/battery_state (sensor_msgs/BatteryState)");
        RCLCPP_INFO(paramNode->get_logger(), "  订阅: /chassis/cmd_vel (geometry_msgs/Twist)");
        RCLCPP_INFO(paramNode->get_logger(), "  TF: odom -> base_link");
        RCLCPP_INFO(paramNode->get_logger(), "  TF: base_link -> imu_link");
        RCLCPP_INFO(paramNode->get_logger(), "===============================================");

        // 创建节点
        auto controlNode = std::make_shared<CRobotControl>("robot_control");
        auto imuNode     = std::make_shared<CRobotImu>("robot_imu");
        auto odomNode    = std::make_shared<CRobotOdometry>("robot_odom");
        auto voltageNode = std::make_shared<CRobotVoltage>("robot_voltage");

        // 使用多线程执行器
        rclcpp::executors::MultiThreadedExecutor executor;
        executor.add_node(controlNode);
        executor.add_node(imuNode);
        executor.add_node(odomNode);
        executor.add_node(voltageNode);

        RCLCPP_INFO(paramNode->get_logger(), "驱动已启动，等待指令...");
        executor.spin();

        rclcpp::shutdown();
    } else {
        RCLCPP_ERROR(paramNode->get_logger(), "机器人失能! (状态码: %d)", enableRobot);
        RCLCPP_ERROR(paramNode->get_logger(), "请检查STM32是否正常工作");
        rclcpp::shutdown();
        return -1;
    }

    return 0;
}
